import java.util.Enumeration;

import javax.media.j3d.Behavior;
import javax.media.j3d.BoundingSphere;
import javax.media.j3d.Transform3D;
import javax.media.j3d.TransformGroup;
import javax.media.j3d.WakeupCondition;
import javax.media.j3d.WakeupOnElapsedFrames;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector3f;

import org.haptiklibrary.HaptikData;
import org.haptiklibrary.HaptikDevice;

/**
 * A simple collision detector class. This responds to a collision event by
 * printing a message with information about the type of collision event and the
 * object involved. This is a variation of the CollisionDetector class that
 * prints information about the object that is associated with this behaviour
 * rather than the object that has been collided with. An example of its use is
 * given in the SimpleCollision2 class.
 * 
 * @author Yi Wu
 * @version 1.0
 * @see CollisionDetector
 * @see SceneNCollision
 */class HandleCollisionDetector extends Behavior {
  /** The shape that is being watched for collisions. */
  protected TransformGroup handleGrp;
  protected TransformGroup puckGrp;
  protected Vector3f handlePosition;
  protected Transform3D handleTrans;
  protected Vector3f puckPosition;
  protected Transform3D puckTrans;
  protected Vector3f V;
  
  protected Transform3D tmpTrans = new Transform3D();
 // protected TransformGroup secondShape;
  
  private HaptikDevice localdevice;
  private HaptikData data = new HaptikData();
  
	private float[] position = new float[3];
	private float[] velocity = new float[3];
	private float[] acceleration = new float[3];
	private float[] force = new float[3];
	private float[] damper = new float[3];

	private Vector3f tmpv;
	private long t = 0L;
	private float dt = 0f;
	private long t_now = 0L;
	
	private float mass = 0.5f;//0.125f;	
	private float damper_coeff = 0.8f;
	
	private float[] scale = new float[3];
	private float[] cali = new float[3];
	
	private int id;
  
  protected WakeupCondition m_WakeupCondition = null;

  /**
   * @param theShape
   *            Shape3D that is to be watched for collisions.
   * @param theBounds
   *            Bounds that define the active region for this behaviour
   */
  public HandleCollisionDetector(TransformGroup handle, TransformGroup puck, HaptikDevice device, Vector3f Velocity, int player) {
    handleGrp = handle;
    puckGrp=puck;
    handleTrans= new Transform3D();
    handlePosition= new Vector3f();
    puckTrans= new Transform3D();
    puckPosition= new Vector3f();
    this.localdevice=device;
    this.V=Velocity;
    id=player;
  //  this.secondShape= sndShape;

  }

  /**
   * This sets up the criteria for triggering the behaviour. It creates an
   * entry, exit and movement trigger, OR's these together and then sets the
   * OR'ed criterion as the wake up condition.
   */
  public void initialize() {	   
	  this.wakeupOn(new WakeupOnElapsedFrames(0));
	  System.out.println( "wake up 0!" );
	  //save the WakeupCriterion for the behavior
	  setSchedulingBounds(new BoundingSphere(new Point3d(0.0d,0.0d,0.0d), 3000.0d));
	  
		scale[0]=4.0f;
		scale[1]=23.0f;;
		scale[2]=5.0f;
		cali[0]=-5;
		cali[1]=+30.0f;
		cali[2]=+60.0f;
	 
  }

  /**
   * This is where the work is done. This identifies the type of collision
   * (entry, exit or movement) and prints a message stating that an object has
   * collided with this object. The userData field of the shape associated
   * with this collision detector # is used to identify the object. Finally,
   * the wake up condition is set to be the OR'ed criterion again.
   */
  public void processStimulus( Enumeration criteria )
  {

    //get the position of the handle
  	data.forceFeedback[0]=0;
	data.forceFeedback[1]=0;
	data.forceFeedback[2]=0;

    puckGrp.getTransform(puckTrans);
    puckTrans.get(puckPosition);

    localdevice.Read(data);
    pollData();
    handlePosition=new Vector3f(position[0],position[1],position[2]);

    //collision criteria
  
  
    if(handlePosition.x>4.1)  //if the handle hit the left wall
  {
  		handlePosition.x=4.1f;
  		data.forceFeedback[0]=-2.0f;  //-0.005f*Math.abs(velocity[0]);
	    //System.out.println( "handle hit the left wall");
  		}
  else if(handlePosition.x<-4.1)  //if the handle hit the right wall
  {
  		handlePosition.x=-4.1f;
  		data.forceFeedback[0]=3.0f;
	 // System.out.println( "handle hit the right wall");
  		}
  if(handlePosition.z>14.1)  //if the handle hit the up wall
  {
  		handlePosition.z=14.1f;
  		data.forceFeedback[2]=-3.0f;
	 // System.out.println( "handle hit the up wall");
	  }
  else if(handlePosition.z<-14.1)  //if the handle hit the bottom wall
  {
  	  handlePosition.z=-14.1f;
  	data.forceFeedback[2]=3.0f;
	 // System.out.println( "handle hit the bottom wall");
  	  }
  
  if(handleGrp.getUserData().equals("player_2")&&handlePosition.z<0)  //if the handle hit the bottom wall
  {
	  handlePosition.z=0;
	 // System.out.println( "player 2 cannot go to the bottom half");
	  }
  else if(handleGrp.getUserData().equals("player_1")&&handlePosition.z>0)  //if the handle hit the bottom wall
  {
	  handlePosition.z=0;
	 // System.out.println( "player 1 cannot go to the top half");
	  }
  if(Math.abs(handlePosition.x)<5.6&&handlePosition.y<1.1&&Math.abs(handlePosition.z)<15.6)  //if the handle go under the table
  {
  	  handlePosition.y=1.1f;
	 // System.out.println( "handle under the table");
  	  }
  if(Math.pow(handlePosition.x-puckPosition.x,2)+Math.pow(handlePosition.z-puckPosition.z,2)<0.64&&handlePosition.y>1.1&&handlePosition.y<1.3)
  {

  		V= computeV(V ,puckPosition, handlePosition);
  		data.forceFeedback[0]=3.0f*Math.abs(V.x)/V.x;
  		data.forceFeedback[2]=3.0f*Math.abs(V.z)/V.z;		
  }
  
  if(id==1)
  {
  	data.forceFeedback[0]=-data.forceFeedback[0];
  	data.forceFeedback[2]=-data.forceFeedback[2];
  }
	  handleTrans.set(handlePosition);
	  handleGrp.setTransform(handleTrans);
	  localdevice.Write(data);
	 

	localdevice.Write(data);

    this.wakeupOn(new WakeupOnElapsedFrames(1));
  }
  
	private void pollData() {
		t_now = System.currentTimeMillis();
		dt = (t_now - t) / 1000.0f;
		t=t_now;
//		System.out.println("f = "+1.0/dt);	
		localdevice.Read(data);	
		
		if (id==2){
		position[0]=(data.position[0]+cali[0])/scale[0];
		position[1]=(data.position[1]+cali[1])/scale[1];
		position[2]=(data.position[2]+cali[2])/scale[2];
		}
		
		if (id==1){
			position[0]=-(data.position[0]+cali[0])/scale[0];
			position[1]=(data.position[1]+cali[1])/scale[1];
			position[2]=-(data.position[2]+cali[2])/scale[2];
			}
		for(int i=0;i<3;i++) {					
			//velocity[i] = (data.position[i]/scale[i]-position[i])/dt;
		
//			position[i]=(data.position[i]+cali[i])/scale[i];	
			
			//System.out.println("position["+i+"]= "+position[i]);
			acceleration[i] = (data.velocity[i]-velocity[i])/dt;			
			velocity[i] = data.velocity[i];			
//			System.out.println("velocity["+i+"]= "+velocity[i]);
			damper[i] = damper_coeff*velocity[i]/scale[i];
			//System.out.println("force["+i+"]= "+force[i]);
		}	
	}
/*	
    private void computeForces() {
		force[0]=0.0f;
		force[1]=0.0f;
		force[2]=0.0f;		
		float temp = 0.0f,temp2=0.0f;
		for(int i=0;i<3;i++) {					
			if(Math.abs(position[i])>2.2f) {
				//System.out.println("yes");
				if (data.position[i]==0.0f)temp=0.0f;
				else temp = 1.0f;
				if(data.velocity[i]==0.0f)temp2=0.0f;
				else temp2= 1.0f;
				float distance = data.position[i]-temp;
				force[i] += -20.0f*distance;
				if(temp2!=temp) {
					damper[i]=damper[i]/20.0f;
				}
				force[i] -= damper[i];
			} else {				
				// do nothing
			}	
			//System.out.println("force["+i+"]= "+force[i]);
		}			
		//force[1]+=1.4f; //force needed to hold device steady ( weight compensation )
		//force[1]+=-mass*9.8f;
	}*/
	private Vector3f computeV(Vector3f OrgV,Vector3f pPos, Vector3f hPos) {
		Vector3f newV= new Vector3f();
		newV.y=0;
		float base= Math.abs(pPos.x-hPos.x)+Math.abs(pPos.z-hPos.z);
		double result1=Math.sqrt(OrgV.x*OrgV.x+OrgV.z*OrgV.z)*(pPos.x-hPos.x)/base;
		newV.x = (float) result1;
		double result2=Math.sqrt(Math.pow(OrgV.x,2)+Math.pow(OrgV.z,2))*(pPos.z-hPos.z)/base;
		newV.z= (float) result2;
		return newV;
	}
	

 }

